#include "RL_SmartCar.h"

void SmartCar::motor_init(){
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);

}


void SmartCar::motor_speed(int id,int speed){
    int pin_a,pin_b;
         switch(id){
            #if defined(ESP32)
            case 0:pin_a=P14;pin_b=P13;break;
            case 1:pin_a=P15;pin_b=P16;break;
            #else
            case 0:pin_a=6;pin_b=5;break;
            case 1:pin_a=9;pin_b=10;break;
            #endif
            default:break;
        }
    if(speed==0){
        #if defined(ESP32)
        analogWrite(pin_a,1023);
        analogWrite(pin_b,1023);
        #else
        analogWrite(pin_a,255);
        analogWrite(pin_b,255);
        #endif
    }
    else if(speed>0){
        #if defined(ESP32)
        analogWrite(pin_a,0);
        analogWrite(pin_b,map(speed, 0, 255, 0, 1023));
        #else
        analogWrite(pin_a,0);
        analogWrite(pin_b,speed);
        #endif
    }
    else{
        #if defined(ESP32)
        analogWrite(pin_b,0);
        analogWrite(pin_a,-map(speed, 0, -255, 0, -1023));
        #else
        analogWrite(pin_b,0);
        analogWrite(pin_a,-speed);
        #endif
    }
}

float SmartCar::car_vol(){
        #if defined(ESP32)
        float vol=analogRead(P1);//*0.0032258;
        #else
        float vol=analogRead(A3);//*0.01466;
        #endif
return vol;
}

int SmartCar::findline(){
    #if defined(ESP32)
    int track_l=analogRead(P2)>100?1:0,track_r=analogRead(P3)>100?1:0;
    #else
    int track_l=analogRead(A6)>100?1:0,track_r=analogRead(A7)>100?1:0;
    #endif

     if (track_l == 1 && track_r == 1) {  // 双探头在线上
        middle = 3;line = 3;last_state = 3;
    } else if (track_l == 0 && track_r == 1) {  // 右边探头在线上
        middle = 1; line = 2; last_state = 2;

    } else if (track_l == 1 && track_r == 0) {  // 左边探头在线上
        middle = 2;line = 4;last_state = 4;

    } else if (track_l == 0 && track_r == 0) {  // 双探头在线外，区分左右
        middle = 0;
        if (last_state == 2||last_state == 0) {  // 上次 1 号探头在线，当前可能左侧线外
         line = 1; 
            } else if (last_state == 4||last_state == 6) {  // 上次 2 号探头在线，当前可能右侧线外
         line = 5; 
            } 
  }
    return line;
}



/*int SmartCar::findline() {

    #if defined(ESP32)
    int track_l=analogRead(P3)>100?1:0,track_r=analogRead(P2)>100?1:0;
    #else
    int track_l=analogRead(A6)>100?1:0,track_r=analogRead(A7)>100?1:0;
    #endif

    TrackState currentState;

    if (track_l == 1 && track_r == 1) {
        currentState = TrackState::BOTH_ON_LINE;
    } else if (track_l == 1 && track_r == 0) {
        currentState = TrackState::LEFT_ON_LINE;
    } else if (track_l == 0 && track_r == 1) {
        currentState = TrackState::RIGHT_ON_LINE;
    } else {
        currentState = TrackState::BOTH_OFF_LINE;
    }

    switch (currentState) {
        case TrackState::BOTH_ON_LINE:
            middle = static_cast<int>(TrackState::BOTH_ON_LINE);
            line = static_cast<int>(LinePosition::BOTH_ON_LINE);
            last_state = static_cast<int>(TrackState::BOTH_ON_LINE);
            wasLeftOffLineLast = false;
            wasRightOffLineLast = false;
            break;
        case TrackState::LEFT_ON_LINE:
            middle = static_cast<int>(TrackState::LEFT_ON_LINE);
            line = static_cast<int>(LinePosition::LEFT_ON_LINE);
            last_state = static_cast<int>(TrackState::LEFT_ON_LINE);
            wasLeftOffLineLast = false;
            wasRightOffLineLast = false;
            break;
        case TrackState::RIGHT_ON_LINE:
            middle = static_cast<int>(TrackState::RIGHT_ON_LINE);
            line = static_cast<int>(LinePosition::RIGHT_ON_LINE);
            last_state = static_cast<int>(TrackState::RIGHT_ON_LINE);
            wasLeftOffLineLast = false;
            wasRightOffLineLast = false;
            break;
        case TrackState::BOTH_OFF_LINE:
            middle = static_cast<int>(TrackState::BOTH_OFF_LINE);
            if (last_state == static_cast<int>(TrackState::LEFT_ON_LINE)) {
                line = static_cast<int>(LinePosition::LEFT_OFF_LINE);
                wasLeftOffLineLast = true;
                wasRightOffLineLast = false;
            } else if (last_state == static_cast<int>(TrackState::RIGHT_ON_LINE)) {
                line = static_cast<int>(LinePosition::RIGHT_OFF_LINE);
                wasLeftOffLineLast = false;
                wasRightOffLineLast = true;
            } else {
                line = static_cast<int>(LinePosition::RIGHT_OFF_LINE);
                wasLeftOffLineLast = false;
                wasRightOffLineLast = true;
            }
            last_state = static_cast<int>(TrackState::BOTH_OFF_LINE);
            break;
    }

    // 处理碰到边界黑线的情况
    if (wasLeftOffLineLast && track_l == 1) {
        line = static_cast<int>(LinePosition::LEFT_BOUNDARY);
    } else if (wasRightOffLineLast && track_r == 1) {
        line = static_cast<int>(LinePosition::RIGHT_BOUNDARY);
    }

    return line;
}
*/

SmartCar::SmartCar() : _track(0x27) {
  // 构造函数体
}

void SmartCar::track_init(){

    _track.begin();
    for(int8_t i=0;i<8;i++){
        _track.pinMode(i, INPUT);
    }
}


void SmartCar::readSensorValues(int *readings) {
    for (int i = 0; i < 8; i++) {
        readings[i] = _track.digitalRead(i);
    }
}

float SmartCar::calculateMiddleValue(int *readings) {
    int edges[7];
    int edgeCount = findTransitionEdges(readings, edges);
    float middleValue = getMiddleValue(edges, edgeCount);

    if (allWhite(readings)) {
        if (_lastMiddleValue != -1) {
            middleValue = _lastMiddleValue;
        }
    } else {
        _lastMiddleValue = middleValue;
    }
    return middleValue;
}

int SmartCar::findTransitionEdges(int *readings, int *edges) {
    int edgeCount = 0;
    for (int i = 1; i < 8; i++) {
        if (readings[i] != readings[i - 1]) {
            edges[edgeCount] = i;
            edgeCount++;
        }
    }
    return edgeCount;
}

float SmartCar::getMiddleValue(int *edges, int edgeCount) {
    if (edgeCount == 0) {
        return -1;
    }
    int sum = 0;
    for (int i = 0; i < edgeCount; i++) {
        sum += edges[i];
    }
    return (float)sum / edgeCount;
}

bool SmartCar::allWhite(int *readings) {
    for (int i = 0; i < 8; i++) {
        if (readings[i] == 1) {
            return false;
        }
    }
    return true;
}

